/*
 * @Description: matching 模块任务管理， 放在类里使代码更清晰
 * @Author: Zhijian Qiao
 * @Date: 2020-02-10 08:31:22
 */
#ifndef avp_slam_MATCHING_MATCHING_FLOW_HPP_
#define avp_slam_MATCHING_MATCHING_FLOW_HPP_

#include <ros/ros.h>
// subscriber
#include "subscriber/four_images_subscriber.hpp"
#include "subscriber/odometry_subscriber.hpp"
#include <tf/transform_listener.h>
// publisher
#include "publisher/odometry_publisher.hpp"
#include "publisher/tf_broadcaster.hpp"
// matching
#include "matching/matching.hpp"
#include "nav_msgs/Path.h"
#include "publisher/path_publisher.hpp"


namespace avp_slam {
    class MatchingFlow {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        MatchingFlow(ros::NodeHandle &nh);

        bool Run();

    private:
        bool ReadData();

        bool HasData();

        bool ValidData();

        bool UpdateMatching();

        bool updatePred();

        bool updateLatestPose();

        bool PublishData();

        Eigen::Matrix4f TransformToMatrix(const tf::StampedTransform &transform);

    private:
        // subscriber
        std::shared_ptr<FourImagesSubscriber> images_sub_ptr_;
        std::shared_ptr<OdometrySubscriber> dr_sub_ptr_;
        std::shared_ptr<OdometrySubscriber> image_pred_sub_ptr_;
        // publisher
        std::shared_ptr<OdometryPublisher> image_odom_pub_ptr_;
        std::shared_ptr<PathPublisher> image_path_pub_ptr_;
        std::shared_ptr<PathPublisher> image_gt_path_pub_ptr_;
        std::shared_ptr<TFBroadCaster> image_tf_pub_ptr_;
        std::shared_ptr<OdometryPublisher> imuVel_odom_pub_ptr_;
        std::shared_ptr<PathPublisher> latest_path_pub_ptr_;
        std::shared_ptr<TFBroadCaster> imuVel_tf_pub_ptr_;

//        listener
        std::shared_ptr<tf::TransformListener> gt_listener_;

        // matching
        std::shared_ptr<Matching> matching_ptr_;

        std::deque<FourImagesData> image_data_buff__;
        std::deque<PoseData> dr_data_buff_;
        std::deque<PoseData> image_pred_data_buff_;

        FourImagesData current_image_data_;
        FourImagesData previous_image_data_;

        Eigen::Matrix4f Twb_map_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f T_pred = Eigen::Matrix4f::Identity();

        bool use_wheel = false;

        std::shared_ptr<TicToc> time_valid;
    };
}

#endif